#include "PID.hpp"

// 限幅函数
int ILimit(int amt, int low, int high)
{
    return std::max(low, std::min(amt, high));
}

float FLimit(float amt, float low, float high)
{
    return std::max(low, std::min(amt, high));
}

// 舵机实时角度计算
float servo_angle(int duty)
{
    return (duty - servo_angle_min) * (angle_img_max - angle_img_min) / servo_angle_max - servo_angle_min - angle_img_max;
}

////////////////////  舵机PID控制 { 带二次项和陀螺仪}  ////////////////////
erciPD::erciPD(float p, float p2, float d, float d2, float i)
    : Kp(p), Kp2(p2), Kd(d), Kd2(d2), Ki(i), prevError(0), integral(0) {}

float erciPD::update(float setpoint, float currentValue, float gyroZ)
{

    float error = setpoint - currentValue;

    integral += error;                      // 积分项累加
    integral = FLimit(integral, -100, 100); // 积分限幅

    float deltaError = error - prevError;

    float output = Kp * error + abs(error) * error * Kp2 + Kd * deltaError + Kd2 * gyroZ + Ki * integral;

    prevError = error;

    return output;
}

////////////////////  舵机串级PID控制   ////////////////////
cascadePID::cascadePID(float p1, float i1, float d1, float p2, float i2, float d2, float k)
    : Kp1(p1), Ki1(i1), Kd1(d1), Kp2(p2), Ki2(i2), Kd2(d2), prevError1(0), prevError2(0), integral1(0), integral2(0), K(k) {}

float cascadePID::update(float setpoint, float currentValue, float gyroZ)
{

    float error1 = setpoint - currentValue;

    integral1 += error1;                      // 积分项累加
    integral1 = FLimit(integral1, -100, 100); // 积分限幅

    float deltaError1 = error1 - prevError1;

    float u1 = Kp1 * error1 + Ki1 * integral1 + Kd1 * deltaError1;

    float error2 = setpoint - u1;

    integral2 += error2;                      // 积分项累加
    integral2 = FLimit(integral2, -100, 100); // 积分限幅

    float deltaError2 = error2 - prevError2;

    float output = Kp2 * error2 + Ki2 * integral2 + Kd2 * deltaError2 + K * gyroZ;

    prevError2 = prevError1;
    prevError1 = error1;

    return output;
}

//////////////////// 电机  ///////////////////////////
incrPIDL::incrPIDL(float p, float i, float d)
: Kp(p), Ki(i), Kd(d), prevError1(0), prevError0(0), integral(0) {}

float incrPIDL::update(float setpoint, float currentValue)
{

    float error = setpoint - currentValue;

    float deltaError = error - prevError1;

    // Δu(k) = Kp[e(k)-e(k-1)] + Ki*e(k) + Kd[e(k)-2e(k-1)+e(k-2)]
    float output = Kp * deltaError +
                Ki * error;

    prevError1 = error;

    return output;
}

incrPIDR::incrPIDR(float p, float i, float d)
: Kp(p), Ki(i), Kd(d), prevError1(0), prevError0(0), integral(0) {}

float incrPIDR::update(float setpoint, float currentValue)
{

    float error = setpoint - currentValue;

    float deltaError = error - prevError1;

    // Δu(k) = Kp[e(k)-e(k-1)] + Ki*e(k) + Kd[e(k)-2e(k-1)+e(k-2)]
        float output = Kp * deltaError +
                       Ki * error;

    prevError1 = error;

    return output;
}